// ==============================================
// =      U.A.V.P Brushless UFO Controller      =
// =           Professional Version             =
// = Copyright (c) 2007 Ing. Wolfgang Mahringer =
// ==============================================
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with this program; if not, write to the Free Software Foundation, Inc.,
//  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// ==============================================
// =  please visit http://www.uavp.org          =
// =               http://www.mahringer.co.at   =
// ==============================================

// Utilities and subroutines

#pragma codepage=1
#include "c-ufo.h"
#include "bits.h"

// Math Library
#include "mymath16.h"

// Limit integral sum of Yaw gyros
// it must be limited to avoid numeric overflow
// which would cause a uncontrolled yawing -> crash
void Sum_and_LimitYawAngle(void)
{

// add the yaw stick value
	Yaw_Rate_8 += IYaw;

	if ( _UseCompass )
	{
// add compass heading correction
// CurDeviation is negative if Ufo has yawed to the right (go back left)

// this double "if" is necessary because of dumb CC5X compiler
		NegFact = YawNeutral + COMPASS_MIDDLE;
		if ( IYaw > NegFact )
// yaw stick is not in neutral zone, learn new desired heading
			AbsDirection = COMPASS_INVAL;
		else		
		{
			NegFact = YawNeutral - COMPASS_MIDDLE;
			if ( IYaw < NegFact )
// yaw stick is not in neutral zone, learn new desired heading
				AbsDirection = COMPASS_INVAL;
			else
			{
// yaw stick is in neutral zone, hold heading
				if( CurDeviation > COMPASS_MAXDEV )
					Yaw_Rate_8 -= COMPASS_MAXDEV;
				else
				if( CurDeviation < -COMPASS_MAXDEV )
					Yaw_Rate_8 += COMPASS_MAXDEV;
				else
					Yaw_Rate_8 -= CurDeviation;
			}
		}
	}


	Yaw_Angle += (long)Yaw_Rate_8;
	NegFact = -YawIntLimit;
	if( (int)Yaw_Angle.high8 >= YawIntLimit )
	{
		Yaw_Angle.high8 = YawIntLimit;
		Yaw_Angle.low8 = 0;
	}
 	if( (int)Yaw_Angle.high8 < NegFact )
	{
		Yaw_Angle.high8 = NegFact;
		Yaw_Angle.low8 = 0;
	}
}

// to avoid stopping motors in the air, the
// motor values are limited to a minimum and
// a maximum

int SaturInt(long l)
{
	if( l > _Maximum )
		return(_Maximum);

	return((int)l);
}

// mix the PID-results (Rl, Nl and Tl) and the throttle
// on the motors and check for numerical overrun
void MixAndLimit(void)
{
	bank2 long min;
	
	if(	BiasZeroCount > 0 ) // wait until the gyro Bias is measured
		return;

#ifdef NO_SENSORS
Rl = Nl = Tl = Vud = VBaroComp = 0;
Rl -= IRoll;
Nl -= IPitch;
Tl -= IYaw;
#endif


#ifndef TRICOPTER
	if( FlyCrossMode )
	{	// "Cross" Mode
// TODO: figure out how the Rl, NL limiting needs to be 	
		Ml = IGas + Nl;		Ml -= Rl;
		Mr = IGas - Nl;		Mr += Rl;
		Mv = IGas - Nl;		Mv -= Rl;
		Mh = IGas + Nl;		Mh += Rl;
	}
	else
	{	// "Plus" Mode
		//The normal way 
		// limit so that substration of  Rl / Nl from Igas can't go below 0
		
		min = (long) IGas;
		
		if ( min  < Rl )
			Rl = min;
		if ( min < Nl )
			Nl = min;
			
		min = -min;
		
		if ( Rl < min )
			Rl = min;
		if ( Nl < min )
			Nl = min;
	
		Ml = IGas - Rl;	// motor Left
		Mr = IGas + Rl; // Motor right
		Mv = IGas - Nl; // Motor front
		Mh = IGas + Nl; // motor rear 
	}

	// Find lowest running motor 

	if (Mv < Mh) 
		min = Mv;
	else
		min = Mh;
	if (Ml < min )
		min = Ml;
	if ( Mr < min )
		min = Mr;
	
	// limit Yaw input so that no motor goes below 0 when added/subtracted	
	if ( min < Tl )
		Tl = min;
		
	min = - min;	
	if (Tl < min )
		Tl = min;

	// Mix in the differential power to the motors for yawing 	
	Mv += Tl;
	Mh += Tl;
	Ml -= Tl;
	Mr -= Tl;
	


// Altitude stabilization factor
// This doesn't work because the Z accel alone can not be used to get up down Velocity whe the craft banks and pitches \
// As the Z axes sees less of the 1G whenever it's not perfectly level. This leads to a negative Velocity  in the end which sends
// the craft Skyward ... 
//	Mv += Vud;
//	Mh += Vud;
//	Ml += Vud;
//	Mr += Vud;
//if (BlinkCount	& 0x4 ) 			
//	SendComValS( (char)Vud);
/*
	Mv += VBaroComp;
	Mh += VBaroComp;
	Ml += VBaroComp;
	Mr += VBaroComp;
*/

#else	// TRICOPTER

// TODO  Same  low gas protection needed as above for + plus mode

	Mv = IGas + Nl;	// front motor
	Ml = IGas + Rl;
	Mr = IGas - Rl;
	Rl >>= 1;
	Ml -= Rl;	// rear left
    Mr -= Nl;	// rear right
	Mh = Tl + _Neutral_Out;	// yaw servo

	if( IGas > MotorLowRun )
	{
		if( (Ml > Mr) && (Mr < MotorLowRun) )
		{
			// Mv += Mh - MotorLowRun
			Ml += Mr;
			Ml -= MotorLowRun;
		}
		if( (Mr > Ml) && (Ml < MotorLowRun) )
		{
			// Mh += Mv - MotorLowRun
			Mr += Ml;
			Mr -= MotorLowRun;
		}
	}
#endif

	// add in the idle 
	Mv += MotorLowRun;
	Mh += MotorLowRun;
	Ml += MotorLowRun;
	Mr += MotorLowRun;
	
	MVorne = SaturInt(Mv);
	MLinks = SaturInt(Ml);
	MRechts = SaturInt(Mr);
	MHinten = SaturInt(Mh);
}

